import numpy as np
import cv2
from  miscfunc import *
from win32api import GetSystemMetrics

"""
获取匹配点对的坐标
"""
def fetchCoordinate(kp1,kp2,matches):
    N=len(matches)
    #首先将匹配点对的坐标提取出来
    pt1=np.zeros((N,2))
    pt2=np.zeros((N,2))
    for i in range(N):
        pt2[i,:]=kp2[matches[i].trainIdx].pt
        pt1[i,:]=kp1[matches[i].queryIdx].pt
    return pt1,pt2

"""
给定点对，相机矩阵，进行三角测量，
得出3D空间点(非齐次)，以及对应相机1和相机2的重投影误差。
"""
def triangulatePoints(pts1,pts2,P,Pp,K):
    N=len(pts1)
    pts1_dir=pts1.reshape([1, N, 2])
    pts1_dir=pts2.reshape([1,N,2])
    pts1_dir=cv2.undistortPoints(pts1_dir,K,None)   #计算K.inv*m，图像点m的反投影射线的方向向量（相机坐标系下）
    pts2_dir=cv2.undistortPoints(pts1_dir,K,None)
    pts1_dir=pts1_dir.reshape([N,2]);     #再转换成2xN的矩阵，每列为一个图像点
    pts2_dir=pts2_dir.reshape([N,2]);
    ##使用opencv的三角测量
    pts3D=cv2.triangulatePoints(P,Pp,pts1_dir.transpose(),pts2_dir.transpose())       ##返回的是4XN矩阵，即N个3D点的齐次坐标
    pts3D=pts3D.transpose()     #得到Nx4,第行为一个3D点
    N=len(pts3D)
    pts3D=cv2.convertPointsFromHomogeneous(pts3D)   ##得到非齐次3D点，Nx3
    pts3D=pts3D.reshape([N,3])
    ##计算到相机1的重投影，以及重投影误差
    rvec,jacobi=cv2.Rodrigues(P[:3,:3])      ##将旋转矩阵用旋转向量表示
    tvec=P[:,3]
    reproj_pts1,jacobi=cv2.projectPoints(pts3D,rvec,tvec,K,None)   ##输出2xN
    reproj_pts1=reproj_pts1.reshape([N,2])
    reproj_err1=reproj_pts1-pts1
    totalMSE1 =np.linalg.norm(reproj_err1, axis=1)
    totalMSE1 =np.sum(totalMSE1)       ##得到相机1重投影的均方误差和
    ##计算到相机2的重投影，以及重投影误差
    rvec,jacobi=cv2.Rodrigues(Pp[:3,:3])
    tvec=P[:,3]
    reproj_pts2,jacobi=cv2.projectPoints(pts3D,rvec,tvec,K,None)        #得到重投影图像点
    reproj_pts2=reproj_pts2.reshape([N,2])  ##Nx2数组
    reproj_err2=reproj_pts2-pts2
    totalMSE2 = np.linalg.norm(reproj_err2,axis=1)
    totalMSE2=np.sum(totalMSE2)       ##得到相机1重投影的均方误差和
    ##
    return pts3D,totalMSE1,totalMSE2

"""
计算F矩阵，并得到inliers
"""
def computeFandPruneMatches(kp1,kp2,matches):
    N=len(matches)
    pt1,pt2=fetchCoordinate(kp1,kp2,matches)
    #计算F矩阵, mask提示哪些匹配是inliers
    F, mask = cv2.findFundamentalMat(pt1,pt2,cv2.FM_RANSAC,1,0.8)      #约0.8的点是正常点
    goodMatches=[matches[i] for i in range(N) if mask[i]]
    #print(F)
    return F,goodMatches

"""
验证3D点是否大部分在相机前方
输入：3D重建点，相机矩阵P（3x4）
返回值：True--验证通过 False--验证失败
"""
def testTriangulation(pts3D,P):
    N=len(pts3D)
    ##将P变为齐次矩阵，4x4
    P=np.vstack((P,[0,0,0,1]))
    ##### perspectiveTransform是原坐标系的3D点变换到相机P的坐标系中的3D点
    projected_pts=cv2.perspectiveTransform(pts3D[None,:,:],P)   #将pts3D变为1xNx3的数组
    projected_pts=projected_pts[0,:,:]  ##去掉外层，将1xNx3的数组变为Nx3
    ##
    positiveZ=[z>0 for (x,y,z) in projected_pts]
    positiveZcnt=np.sum(positiveZ)
    percentage=positiveZcnt/N       #相机前方点占所有3D点的比率
    if percentage<0.75:
        return False

    return True


"""
计算camera1,camera2的相机投影矩阵
返回：res,P,Pp(即P')
"""
def computeCameraMatrix(pts1,pts2,F,K):
    M=len(pts1)     ##点的个数
    P = np.array([[1, 0, 0, 0],
                  [0, 1, 0, 0],
                  [0, 0, 1, 0]],dtype=np.float)
    E=np.dot(K.transpose(),np.dot(F,K))     ##E=K^t*F*K
    if(np.fabs(np.linalg.det(E)) >1e-07):       #E的秩理论上为2，所以det(E)理论上应等于零
        return False,None,None
    ##从E矩阵分解得到R和t
    res,R1,R2,t1,t2=decomposeE2RandT(E)
    if not res:
        return False,None,None
    ##检查R1
    if np.linalg.det(R1)<=0:      ##参见Essential Matrix的wikipedia==>Showing that it is valid
        E=-E;
        res,R1,R2,t1,t2=decomposeE2RandT(E)
    if not res:
        return False,None,None
    if np.fabs(np.linalg.det(R1)-1.0)>1e-7:     ##det(R1)不等于1
        return False,None,None
    ############使用R1,t1作为投影矩阵参数，进行三角测量，得到3D点（投影重建）
    Pp=np.hstack((R1,t1.reshape(3,1)))      #这里的P还只是相机外参
    pts3D,reproj_err1,reproj_err2=triangulatePoints(pts1,pts2,P,Pp,K)        ##三角测量，得到投影重建3D点，以及两个相机的重投影误差
    ##得到的pts3D是Nx3结构的非齐次点
    ##验证由R1,t1得到的投影重建3D点是否在相机2的前方
    # (因为相机1不是计算得出因是先前就固定的，所以3D点肯定是在前方的)。
    isOK=testTriangulation(pts3D,Pp)
    if isOK and reproj_err1<100 and reproj_err2<100:
        return True,P,Pp,pts3D
    #############如果R1,t1未通过，则取R1,t2
    Pp = np.hstack((R1, t2.reshape(3, 1)))
    pts3D, reproj_err1, reproj_err2 = triangulatePoints(pts1, pts2, P, Pp, K)
    isOK=testTriangulation(pts3D,Pp)
    if isOK and reproj_err1<100 and reproj_err2<100:
        return True,P,Pp,pts3D
    #############如果R1,t2未通过，则取R2,t1
    Pp = np.hstack((R2, t1.reshape(3, 1)))
    pts3D, reproj_err1, reproj_err2 = triangulatePoints(pts1, pts2, P, Pp, K)
    isOK=testTriangulation(pts3D,Pp)
    if isOK and reproj_err1<100 and reproj_err2<100:
        return True,P,Pp,pts3D
    #############如果R2,t1未通过，则取R2,t2
    Pp = np.hstack((R2, t2.reshape(3, 1)))
    pts3D, reproj_err1, reproj_err2 = triangulatePoints(pts1, pts2, P, Pp, K)
    isOK=testTriangulation(pts3D,Pp)
    if isOK and reproj_err1<100 and reproj_err2<100:
        return True,P,Pp,pts3D

    return False,None,None,None